#
# Copyright (c) 2018-2022 Michele Segata <segata@ccs-labs.org>
#
# This program is free software: you can redistribute it and/or modify
# it under the terms of the GNU Lesser General Public License as published by
# the Free Software Foundation, either version 3 of the License, or
# (at your option) any later version.
#
# This program is distributed in the hope that it will be useful,
# but WITHOUT ANY WARRANTY; without even the implied warranty of
# MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
# GNU Lesser General Public License for more details.
#
# You should have received a copy of the GNU Lesser General Public License
# along with this program.  If not, see http://www.gnu.org/licenses/.
#

import sys
import os
import random
import math

# vehicle length
LENGTH = 4
# inter-vehicle distance
DISTANCE = 15
# cruising speed
SPEED = 20

LEADER = "p.0"
N_VEHICLES = 4
if 'SUMO_HOME' in os.environ:
    tools = os.path.join(os.environ['SUMO_HOME'], 'tools')
    sys.path.append(tools)
else:
    sys.exit("please declare environment variable 'SUMO_HOME'")
import uuid
import sumolib
import traci
from plexe import Plexe, ACC, CACC, GEAR, RPM
from plexe import POS_X, POS_Y, ENGINE_MODEL_REALISTIC
from os.path import join
import rou.constants as rc
# lane change state bits
bits = {
    0: 'LCA_NONE',
    1 << 0: 'LCA_STAY',
    1 << 1: 'LCA_LEFT',
    1 << 2: 'LCA_RIGHT',
    1 << 3: 'LCA_STRATEGIC',
    1 << 4: 'LCA_COOPERATIVE',
    1 << 5: 'LCA_SPEEDGAIN',
    1 << 6: 'LCA_KEEPRIGHT',
    1 << 7: 'LCA_TRACI',
    1 << 8: 'LCA_URGENT',
    1 << 9: 'LCA_BLOCKED_BY_LEFT_LEADER',
    1 << 10: 'LCA_BLOCKED_BY_LEFT_FOLLOWER',
    1 << 11: 'LCA_BLOCKED_BY_RIGHT_LEADER',
    1 << 12: 'LCA_BLOCKED_BY_RIGHT_FOLLOWER',
    1 << 13: 'LCA_OVERLAPPING',
    1 << 14: 'LCA_INSUFFICIENT_SPACE',
    1 << 15: 'LCA_SUBLANE',
    1 << 16: 'LCA_AMBLOCKINGLEADER',
    1 << 17: 'LCA_AMBLOCKINGFOLLOWER',
    1 << 18: 'LCA_MRIGHT',
    1 << 19: 'LCA_MLEFT',
    1 << 30: 'LCA_UNKNOWN'
}

# 构造 platoon，返回 platoon 中车辆之间的通信连接关系
def add_vehicles(plexe, n, n_platoons, position,platoonSwitch, real_engine=False):
    """
    Adds a set of platoons of n vehicles each to the simulation
    :param plexe: API instance
    :param n: number of vehicles of the platoon
    :param n_platoons: number of platoons
    :param real_engine: set to true to use the realistic engine model,
    false to use a first order lag model
    :return: returns the topology of the platoon, i.e., a dictionary which
    indicates, for each vehicle, who is its leader and who is its front
    vehicle. The topology can the be used by the data exchange logic to
    automatically fetch data from leading and front vehicle to feed the CACC
    """
    # add a platoon of n vehicles
    # topology 中以 dictionary 的形式存储车辆之间的通信拓扑结构，包括每辆车的前车和头车编号
    topology = {}
    p_length = n * LENGTH + (n - 1) * DISTANCE
    for p in range(n_platoons):
        for i in range(n):
            vid = "v.%d.%d" % (p, i)
            # 调用 utils module 中的函数，将车辆编排成 platoon 的形式
            add_platooning_vehicle(plexe, vid, position - i * (DISTANCE + LENGTH), 3, SPEED, DISTANCE,platoonSwitch, real_engine)
            # add_platooning_vehicle(plexe, vid, position, 3, SPEED, DISTANCE,platoonSwitch, real_engine)

            # 将车辆保持在 lane 0 ，并忽略安全距离限制
            # plexe.set_fixed_lane(vid, 3, False)

            # 设置车辆速度模式。车辆的速度有 5 个影响因素
            # https://sumo.dlr.de/wiki/TraCI/Change_Vehicle_State#speed_mode_.280xb3.29
            # 1\. safe speed
            # 2\. max accel
            # 3\. max speed
            # 4\. right of way at intersections
            # 5\. brake hard to avoid passing red light
            # 如果全都不考虑，则设置为 [0, 0, 0, 0, 0] = 0, 此时，车速完全由 traci 控制
            # 如果全都考虑，则设置为 [1, 1, 1, 1, 1] = 31
            # 如果只考虑 safe speed，则设置为 [0, 0, 0, 0, 1] = 1
            traci.vehicle.setSpeedMode(vid, 0)

            # 在 platoon 中头车采用 adaptive cruise control 的控制方式
            # 后边的跟车采用 cooperative adative cruise control 的控制方式
            plexe.use_controller_acceleration(vid, False)
            if i == 0:
                plexe.set_active_controller(vid, ACC)
            else:
                plexe.set_active_controller(vid, CACC)

            if i > 0:
                topology[vid] = {"front": "v.%d.%d" % (p, i - 1), "leader": "v.%d.0" % p}
            else:
                topology[vid] = {}
    return topology
# 路线计时器
routeCount=0
plexe_routeId=None
route_start=None
route_end=None
def add_vehicle(plexe, vid, position, lane, speed,platoonSwitch, vtype="0"):
    global routeCount,plexe_routeId,route_start,route_end

    if plexe.version[0] >= 1:

        if platoonSwitch['type']==0 or platoonSwitch['type']==2:
            traci.vehicle.add(vid, "platoon_route", departPos=str(position),
                          departSpeed='2', departLane=str(lane),
                          typeID=vtype)

        elif platoonSwitch['type']==1:
            if routeCount==0:
                plexe_routeId,route_start,route_end=add_plexe_route(platoonSwitch['lat'],platoonSwitch['lon'])
                routeCount=1
            traci.vehicle.add(vid, plexe_routeId, departPos=str(position),
                              departSpeed='2', departLane=str(lane),
                              typeID=vtype)
              # 路网文件
    else:
        traci.vehicle.add(vid, "platoon_route", pos=position, speed=speed,
                          lane=lane, typeID=vtype)
def add_plexe_route(lat,lon):
    default_path = rc.DEFAULT_PATH
    sumo_net_path = join(default_path, "6e2fccc4-f09d-411e-93ba-407b6f70c0f9.net.xml")
    sumo_net = sumolib.net.readNet(sumo_net_path, withInternal=True, withLatestPrograms=True)
    startedgeID, startpos, startlaneIndex = traci.simulation.convertRoad(lon, lat, True)
    single_edge = sumo_net.getEdge(startedgeID)
    if single_edge is not None:
        temp = [startedgeID]
        count = 0
        while single_edge is not None:
            edge_Bound = single_edge.getOutgoing()
            edgeids = list()
            for k, v in edge_Bound.items():
                if '-361.0.00' in k.getID() or '-254.0.00'  in k.getID() or '371.0.00'  in k.getID():
                    continue
                edgeids.append(k.getID())
            if edgeids is not None and len(edgeids) > 0:
                try:
                    id = random.choice(edgeids)
                    if True:
                        temp.append(id)
                    else:
                        logging.info("Road marking problem1")
                        errorMsg = "标注错误，请重新标注"
                    single_edge = sumo_net.getEdge(id)
                except ValueError:
                    continue
                # break
            count += 1
            if len(temp) > 1000 or count > 200 or '-547.0.00' in temp:
                print(temp)
                break
        routeid = str(uuid.uuid1())
        traci.route.add(routeid, temp)
        route_start=temp[0]
        route_end=temp[-1]
        return routeid,route_start,route_end
population = {"route1", "route2"}
weights = [("route1", 0.5), ("route2", 0.5)]
population_type = {1, 3, 15}
weights_type = [(1, 0.5), (3, 0.05), (15, 0.45)]
def add_vehicle1(plexe, vid, position, lane, speed, vtype="1"):
    element = random.choices(population=list(population), weights=[w[1] for w in weights], k=1)[0]
    element_type = random.choices(population=list(population_type), weights=[w[1] for w in weights_type], k=1)[0]
    if plexe.version[0] >= 1:
        print(element)
        traci.vehicle.add(vid, element, departPos=str(position),
                          departSpeed=str(speed), departLane=str(lane),
                          typeID=str(element_type))
    else:
        traci.vehicle.add(vid, "route1", pos=position, speed=speed,
                          lane=lane, typeID=vtype)


def add_platooning_vehicle(plexe, vid, position, lane, speed, cacc_spacing,
                           platoonSwitch,real_engine=False, vtype="0"):
    """
    Adds a vehicle to the simulation
    :param plexe: API instance
    :param vid: vehicle id to be set
    :param position: position of the vehicle
    :param lane: lane
    :param speed: starting speed
    :param cacc_spacing: spacing to be set for the CACC
    :param real_engine: use the realistic engine model or the first order lag
    model
    """
    add_vehicle(plexe, vid, position, lane, speed,platoonSwitch, vtype)

    plexe.set_path_cacc_parameters(vid, cacc_spacing, 2, 1, 0.5)
    plexe.set_cc_desired_speed(vid, speed)
    # 时间间隔，即跟随前方车辆的最小时间间隔
    plexe.set_acc_headway_time(vid, 1.5)
    if real_engine:
        plexe.set_engine_model(vid, ENGINE_MODEL_REALISTIC)
        plexe.set_vehicles_file(vid, "vehicles.xml")
        plexe.set_vehicle_model(vid, "alfa-147")
    traci.vehicle.setColor(vid, (random.uniform(0, 255),
                                 random.uniform(0, 255),
                                 random.uniform(0, 255), 255))


def get_distance(plexe, v1, v2):
    """
    Returns the distance between two vehicles, removing the length
    :param plexe: API instance
    :param v1: id of first vehicle
    :param v2: id of the second vehicle
    :return: distance between v1 and v2
    """
    v1_data = plexe.get_vehicle_data(v1)
    v2_data = plexe.get_vehicle_data(v2)
    return math.sqrt((v1_data[POS_X] - v2_data[POS_X])**2 +
                     (v1_data[POS_Y] - v2_data[POS_Y])**2) - 4


def communicate(plexe, topology):
    """
    Performs data transfer between vehicles, i.e., fetching data from
    leading and front vehicles to feed the CACC algorithm
    :param plexe: API instance
    :param topology: a dictionary pointing each vehicle id to its front
    vehicle and platoon leader. each entry of the dictionary is a dictionary
    which includes the keys "leader" and "front"
    """
    for vid, l in topology.items():
        if "leader" in l.keys():
            # get data about platoon leader
            ld = plexe.get_vehicle_data(l["leader"])
            # pass leader vehicle data to CACC
            plexe.set_leader_vehicle_data(vid, ld)
            # pass data to the fake CACC as well, in case it's needed
            plexe.set_leader_vehicle_fake_data(vid, ld)
        if "front" in l.keys():
            # get data about platoon leader
            fd = plexe.get_vehicle_data(l["front"])
            # pass front vehicle data to CACC
            plexe.set_front_vehicle_data(vid, fd)
            # compute GPS distance and pass it to the fake CACC
            distance = get_distance(plexe, vid, l["front"])
            plexe.set_front_vehicle_fake_data(vid, fd, distance)


def start_sumo(config_file, already_running, gui=True):
    """
    Starts or restarts sumo with the given configuration file
    :param config_file: sumo configuration file
    :param already_running: if set to true then the command simply reloads
    the given config file, otherwise sumo is started from scratch
    :param gui: start GUI or not
    """
    arguments = ["--step-length", "0.1","--lanechange.duration", "0.25", "-c"]
    sumo_cmd = [sumolib.checkBinary('sumo-gui' if gui else 'sumo')]
    arguments.append(config_file)
    if already_running:
        traci.load(arguments)
    else:
        sumo_cmd.extend(arguments)
        traci.start(sumo_cmd)


def running(demo_mode, step, max_step):
    """
    Returns whether the demo should continue to run or not. If demo_mode is
    set to true, the demo should run indefinitely, so the function returns
    true. Otherwise, the function returns true only if step <= max_step
    :param demo_mode: true if running in demo mode
    :param step: current simulation step
    :param max_step: maximum simulation step
    :return: true if the simulation should continue
    """
    if demo_mode:
        return True
    else:
        return step <= max_step


def get_status(status):
    """
    Returns a human readable representation of the lane change state of a
    vehicle
    :param status: the lane change state returned by getLaneChangeState
    """
    st = ""
    for i in range(32):
        mask = 1 << i
        if status & mask:
            if mask in bits.keys():
                st += " " + bits[mask]
            else:
                st += " 2^" + str(i)
    return st
def get_station_number():
    # 获取路网拓扑信息
    net = traci.simulation.getNet()
    net.getPoidList
